﻿#region Copyright
// Copyright (c) 2011-2012 RaisingForce Team (Alikin Sergey)
//
// Permission is hereby granted, free of charge, to any person
// obtaining a copy of this software and associated documentation
// files (the "Software"), to deal in the Software without
// restriction, including without limitation the rights to use,
// copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following
// conditions:
//
// The above copyright notice and this permission notice shall be
// included in all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
// OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
// WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
// OTHER DEALINGS IN THE SOFTWARE.
#endregion

using System;
using System.Diagnostics.Contracts;
using System.Threading;
using NKH.MindSqualls;
using NKH.MindSqualls.MotorControl;
using RaisingForce.Raise.Measures;

namespace RaisingForce.Raise.Hardware.LegoTribot
{
    sealed internal class LegoTribotEngine :
        RobotEngine
    {
        #region Constructors

        public LegoTribotEngine(
            object hardwareSyncObject,
            object robotSyncObject,
            NxtMotor motorLeft,
            NxtMotor motorRight,
            IRobotEngineOptions options)
            : base(hardwareSyncObject)
        {
            Contract.Requires(hardwareSyncObject != null);
            Contract.Requires(robotSyncObject != null);
            Contract.Requires(motorLeft != null);
            Contract.Requires(motorRight != null);
            Contract.Requires(options != null);
            this.robotSyncObject = robotSyncObject;
            this.motorLeft = motorLeft;
            this.motorRight = motorRight;
            this.motorSync = new McNxtMotorSync(motorLeft, motorRight);
            this.options = options;
        }

        #endregion

        #region Private Declarations

        private readonly NxtMotor motorLeft;

        private readonly NxtMotor motorRight;

        private readonly NxtMotorSync motorSync;

        private readonly IRobotEngineOptions options;

        private readonly object robotSyncObject;

        private const int waitIntervalInMilliseconds = 250;

        private static void pollMotor(NxtMotor motor)
        {
            Contract.Requires(motor != null);
            try
            {
                motor.Poll();
            }
            catch (Exception ex)
            {
                throw new RobotHardwareException(
                    RobotHardwareFailureReason.Hardware,
                    RobotHardwareExceptionMessages.EnginePollError,
                    innerException: ex);
            }
        }

        private void waitUntilMovementEnds()
        {
            // wait for wheels to run
            Thread.CurrentThread.Join(LegoTribotEngine.waitIntervalInMilliseconds);
            int? prevLeftTacho = null;
            int? prevRightTacho = null;
            bool movementProceeds;
            do
            {
                lock (this.robotSyncObject)
                {
                    movementProceeds = false;
                    // detect movement for left wheel
                    LegoTribotEngine.pollMotor(this.motorLeft);
                    int? leftTacho = this.motorLeft.TachoCount;
                    if (leftTacho != prevLeftTacho)
                    {   // left wheel moves
                        prevLeftTacho = leftTacho;
                        // detect movement for right wheel
                        LegoTribotEngine.pollMotor(this.motorRight);
                        int? rightTacho = this.motorRight.TachoCount;
                        if (rightTacho != prevRightTacho)
                        {   // right wheel moves
                            prevRightTacho = rightTacho;
                            // wait for a while
                            movementProceeds = true;
                        }
                    }
                }
                if (movementProceeds)
                {
                    Thread.CurrentThread.Join(
                        LegoTribotEngine.waitIntervalInMilliseconds);
                }
            } while (movementProceeds);
            // additional wait interval for robot
            // before hardware methods are called by the user again
            Thread.CurrentThread.Join(LegoTribotEngine.waitIntervalInMilliseconds);
        }

        #endregion

        #region Base Overrides

        protected override void RotateHardware(
            Angle rotationAngle,
            RotationDirection rotationDirection)
        {
            sbyte leftWheelPower, rightWheelPower;
            this.options.GetWheelsPower(out leftWheelPower, out rightWheelPower);
            ushort fullTurnTacho;
            switch (rotationDirection)
            {
                case RotationDirection.Clockwise:
                    fullTurnTacho = this.options.RightTurnTacho;
                    leftWheelPower *= -1;
                    break;
                case RotationDirection.CounterClockwise:
                default:
                    fullTurnTacho = this.options.LeftTurnTacho;
                    rightWheelPower *= -1;
                    break;
            }
            double angleInDegree = rotationAngle
                .GetNormalized()
                .Get(AngleMeasures.Degree);
            ushort tachoLimit = (ushort)Math.Round(angleInDegree / 360 * fullTurnTacho);
            lock (this.robotSyncObject)
            {   // exceptions are handled in base class
                this.motorLeft.Run(leftWheelPower, tachoLimit);
                this.motorRight.Run(rightWheelPower, tachoLimit);
            }
            this.waitUntilMovementEnds();
        }

        protected override void MoveHardware(
            Distance movementDistance,
            MovementDirection movementDirection)
        {
            sbyte leftWheelPower, rightWheelPower;
            this.options.GetWheelsPower(out leftWheelPower, out rightWheelPower);
            ushort tachoLimit = (ushort)Math.Round(((
                movementDistance.Get(DistanceMeasures.Centimetre)
                / this.options.WheelRunDistance.Get(DistanceMeasures.Centimetre)) * 360));
            switch (movementDirection)
            {
                case MovementDirection.Forward:
                    // do nothing
                    break;
                case MovementDirection.Backward:
                    leftWheelPower *= (sbyte)-1;
                    rightWheelPower *= (sbyte)-1;
                    break;
                default:
                    throw new NotSupportedException(
                        string.Format(
                            "Movement direction not supported: {0}.",
                            movementDirection.ToString()));
            }
            lock (this.robotSyncObject)
            {   // exceptions are handled in base class
                if (leftWheelPower == rightWheelPower)
                {
                    this.motorSync.Run(leftWheelPower, tachoLimit, turnRatio: 0);
                }
                else
                {
                    this.motorLeft.Run(leftWheelPower, tachoLimit);
                    this.motorRight.Run(rightWheelPower, tachoLimit);
                }
            }
            this.waitUntilMovementEnds();
        }

        protected override void StopHardware()
        {
            NxtMotorSync motorSync = this.motorSync;
            if (motorSync != null)
            {
                lock (this.robotSyncObject)
                {   // exceptions are handled in base class
                    motorSync.Brake();
                }
                this.waitUntilMovementEnds();
            }
        }

        #endregion
    }
}
